#include "Ikine_Task.h"
#include "robotics.h"
#include "matrix.h"

void ikine_Task(void)
{
    float m[6] = {0.2645, 0.17, 0.1705, 0, 0, 0};

    float tmp1[18] = {0, -8.5e-2, 0, 0, 0, 0,
                      13.225e-2, 0, 0, 0, 0, 0,
                      0, 3.7e-2, 8.525e-2, 0, 0, 0};
    Matrixf<3, 6> rc(tmp1);
    Matrixf<3, 3> I[6];

    float tmp2[3] = {1.542e-3, 0, 1.542e-3};
    float tmp3[3] = {0, 0.409e-3, 0.409e-3};
    float tmp4[3] = {0.413e-3, 0.413e-3, 0};
    I[0] = matrixf::diag<3, 3>(tmp2);
    I[1] = matrixf::diag<3, 3>(tmp3);
    I[2] = matrixf::diag<3, 3>(tmp4);
    I[3] = matrixf::eye<3, 3>() * 3.0f;
    I[4] = matrixf::eye<3, 3>() * 2.0f;
    I[5] = matrixf::eye<3, 3>() * 1.0f;

    robotics::Link links[6];
    links[0] = robotics::Link(0, 26.45e-2, 0, -PI / 2, robotics::R, 0, 0, 0, m[0], rc.col(0), I[0]);
    links[1] = robotics::Link(0, 5.5e-2, 17e-2, 0, robotics::R, 0, 0, 0, m[1], rc.col(1), I[1]);
    links[2] = robotics::Link(0, 0, 0, -PI / 2, robotics::R, 0, 0, 0, m[2], rc.col(2), I[2]);
    links[3] = robotics::Link(0, 17.05e-2, 0, PI / 2, robotics::R, 0, 0, 0, m[3], rc.col(3), I[3]);
    links[4] = robotics::Link(0, 0, 0, -PI / 2, robotics::R, 0, 0, 0, m[4], rc.col(4), I[4]);
    links[5] = robotics::Link(0, 0, 0, 0, robotics::R, 0, 0, 0, m[5], rc.col(5), I[5]);
    robotics::Serial_Link<6> p560(links);

    float q[6] = {0.2, -0.5, -0.3, -0.6, 0.5, 0.2};
    float qv[6] = {1, 0.5, -1, 0.3, 0, -1};
    float qa[6] = {0.2, -0.3, 0.1, 0, -1, 0};
    float he[6] = {1, 2, -3, -0.5, -2, 1};

    Matrixf<4, 4> T = p560.fkine(q);
    Matrixf<6, 6> J = p560.jacob(q);

    float tmp5[6] = {0, 0, 0, 0, 0.1, 0};
    Matrixf<6, 1> q_ikine = p560.ikine(T, Matrixf<6, 1>(tmp5));
    Matrixf<6, 1> torq = p560.rne(q, qv, qa, he);

    /* Example 2: UR -----------------------------------------------------------------------------*/
    //    robotics::Link links[6];
    //    links[0] = robotics::Link(0, 0, 0, PI / 2, robotics::R, 0);
    //    links[1] = robotics::Link(0, 0, 5, 0, robotics::R, PI / 2);
    //    links[2] = robotics::Link(0, 0, 5, 0, robotics::R, -PI / 2);
    //    links[3] = robotics::Link(0, 1, 0, -PI / 2, robotics::R, 0);
    //    links[4] = robotics::Link(0, 1, 0, -PI / 2, robotics::R, -PI / 2);
    //    links[5] = robotics::Link(0, 1, 0, 0, robotics::R, 0);
    //    robotics::Serial_Link<6> UR(links);

    //    float q[6] = {0.1, 0.2, 0.3, 0.4, 0.5, 0.6};
    //    Matrixf<4, 4> T = UR.fkine(q);
    //    Matrixf<6, 6> J = UR.jacob(q);
    //    Matrixf<6, 1> q_ikine = UR.ikine(T);

    /* Example 3: SCARA --------------------------------------------------------------------------*/
    //    robotics::Link links[4];
    //    links[0] = robotics::Link(0, 5, 5, 0);
    //    links[1] = robotics::Link(0, 0, 5, 0);
    //    links[2] = robotics::Link(0, 0, 0, PI);
    //    links[3] = robotics::Link(0, 0, 0, 0, robotics::P);
    //    robotics::Serial_Link<4> SCARA(links);

    //    float q[4] = {0.2, 0.5, 0.3, 3};
    //    Matrixf<4, 4> T = SCARA.fkine(q);
    //    Matrixf<6, 4> J = SCARA.jacob(q);
    //    Matrixf<4, 1> q_ikine = SCARA.ikine(T, Matrixf<4, 1>((float[4]){0, 0, 0, 0}));

    //    // QR & SVD, unfinished in current version
    //    // QR<6, 4> J_qr(J);
    //    // Matrixf<6, 6> Q = J_qr.Q();
    //    // Matrixf<6, 4> R = J_qr.R();
    //    // SVD<6, 4> J_svd(J);

}



